﻿using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Shapes;

namespace FRC_Turtle.ViewModels
{
    public class TGPoint : BindableBase
    {
        public TGPoint(double x, double y)
        {
            this.X = x;
            this.Y = y;
        }

        private double _x;
        public double X
        {
            get { return _x; }
            set
            {
                _x = value;
                this.OnPropertyChanged();
            }
        }

        private double _y;
        public double Y
        {
            get { return _y; }
            set
            {
                _y = value; this.OnPropertyChanged();
            }
        }

        private double _shotHead;
        public double ShotHeading
        {
            get { return _shotHead; }
            set
            {
                _shotHead = value; this.OnPropertyChanged();
            }
        }
        
        private int shotCount;
        public int ShotCount
        {
            get { return shotCount; }
            set
            {
                shotCount = value;
                this.OnPropertyChanged();
            }
        }

        public override string ToString()
        {
            StringBuilder sb = new StringBuilder();

            sb.Append("X: " + Math.Round(X, 2) + " Y: " + Math.Round(Y, 2));

            if (ShotCount == 1)
            {
                sb.Append(" Shot at: " + Math.Round(ShotHeading, 2));
            }
            else if (ShotCount > 2)
            {
                sb.Append(" " + ShotCount + " shots at: " + Math.Round(ShotHeading, 2));
            }

            return sb.ToString();
        }
    }

    class FRCTurtleViewModel : BindableBase
    {
        public FRCTurtleViewModel()
        {
            Points.CollectionChanged += (s, e) => this.OnPropertyChanged("Segments");
        }

        public ObservableCollection<TGPoint> Points
        {
            get { return (ObservableCollection<TGPoint>)GetValue(PointsProperty); }
            set { SetValue(PointsProperty, value); }
        }

        private static void PointsChanged(DependencyObject sender, DependencyPropertyChangedEventArgs e)
        {
            (sender as FRCTurtleViewModel).OnPropertyChanged("Segments");
        }

        // Using a DependencyProperty as the backing store for Points.  This enables animation, styling, binding, etc...
        public static readonly DependencyProperty PointsProperty =
            DependencyProperty.Register("Points", typeof(ObservableCollection<TGPoint>), typeof(FRCTurtleViewModel), new PropertyMetadata(new ObservableCollection<TGPoint>(), new PropertyChangedCallback(PointsChanged)));


        public const double FieldPixWidth = 1392;
        public const double FieldPixHeight = 779;


        public const double FieldWidth = 16.46;
        public const double FieldHeight = 8.23;
        public const double RobotSpeed = 2;

        public class Segment
        {
            public double X1 { get; set; }
            public double Y1 { get; set; }
            public double X2 { get; set; }
            public double Y2 { get; set; }

            public double pX1
            {
                get
                {
                    return this.X1 * FRCTurtleViewModel.FieldPixWidth / FieldWidth;
                }
            }
            public double pY1
            {
                get
                {
                    return this.Y1 * FRCTurtleViewModel.FieldPixHeight / FieldHeight;
                }
            }
            public double pX2
            {
                get
                {
                    return this.X2 * FRCTurtleViewModel.FieldPixWidth / FieldWidth;
                }
            }
            public double pY2
            {
                get
                {
                    return this.Y2 * FRCTurtleViewModel.FieldPixHeight / FieldHeight;
                }
            }

            public double Heading
            {
                get
                {
                    double head = (Math.Atan2(Y1 - Y2, X2 - X1) * -180 / Math.PI + 90);

                    if (head < 0)
                        head = 360 + head;

                    return head % 360;
                }
            }

            public override string ToString()
            {
                return "Head = " + Heading;
            }

            public double Length
            {
                get
                {
                    double x = X1 - X2;
                    double y = Y1 - Y2;
                    return Math.Sqrt(x * x + y * y);
                }
            }
        }

        public string BuildScript()
        {
            var segs = this.Segments.ToList();

            StringBuilder sb = new StringBuilder();

            double lastHeading = 0;

            double time = 0;

            for (int i = 0; i < segs.Count; i++)
            {
                var seg = segs[i];
                double headingDiff = seg.Heading - lastHeading;
                headingDiff %= 180;
                lastHeading = seg.Heading;
                sb.Append(time + ": rotate(" + headingDiff + ");\n");
                time += 2.5f;
                sb.Append(time + ": drive(" + seg.Length + ");\n");
                time += seg.Length / RobotSpeed;
            }

            sb.Append(time + ": return();\n");

            return sb.ToString();
        }

        public IEnumerable<Segment> Segments
        {
            get
            {
                var p = this.Points;

                if (p.Count < 2)
                    yield break;

                var last = p[0];

                for (int i = 1; i < Points.Count; i++)
                {
                    Segment s = new Segment();

                    s.X1 = last.X;
                    s.Y1 = last.Y;
                    s.X2 = p[i].X;
                    s.Y2 = p[i].Y;

                    yield return s;

                    last = p[i];
                }
            }
        }
    }
}
